import copy
import numpy as np

#-----disabled for pytests-----
#from datetime import datetime, timedelta
#import subprocess
#from openpilot.common.time_helpers import system_time_valid
#from openpilot.common.swaglog import cloudlog

from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser

from opendbc.car.common.conversions import Conversions as CV
#from opendbc.car.common.numpy_fast import mean
from opendbc.car import Bus, create_button_events, structs
from opendbc.car.interfaces import CarStateBase
from opendbc.car.byd.values import DBC, CanBus, LKASConfig, CarControllerParams

ButtonType = structs.CarState.ButtonEvent.Type

class CarState(CarStateBase):
  def __init__(self, CP):
    super().__init__(CP)

    can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])

    self.shifter_values = can_define.dv["DRIVE_STATE"]["Gear"]

    self.speed_kph = 0

    self.mpc_lkas_config = 0

    self.acc_hud_adas_counter = 0
    self.acc_mpc_state_counter = 0
    self.acc_cmd_counter = 0

    self.eps_warning = False

    self.acc_active_last = False
    self.low_speed_alert = False
    self.lkas_allowed_speed = False

    self.lkas_prepared = False  #318, EPS to OP
    self.acc_state = 0
    self.adas_set_dist = 0

    self.mpc_laks_output = 0
    self.mpc_laks_active = False
    self.mpc_laks_reqprepare = False

    #self.prev_angle = 0

    self.cam_lkas = 0
    self.cam_acc = 0
    self.esc_eps = 0

    self.setTimeDelay = 100

    self.mrr_leading_dist = 0

    self.btn_acc_cancel = 0
    self.btn_acc_set_reset = 0
    self.btn_acc_dist_inc = 0
    self.btn_acc_dist_dec = 0

    self.prev_steeringAngleDeg = 0
    #self.steeringRate = 0.0
    self.steeringRateDegAbs = 0



  def update(self, can_parsers) -> structs.CarState: # type: ignore
    cp = can_parsers[Bus.pt]
    cp_cam = can_parsers[Bus.cam]

    ret = structs.CarState()

    self.lkas_prepared = cp.vl["ACC_EPS_STATE"]["LKAS_Prepared"]

    self.mpc_lkas_config = int(cp_cam.vl["ACC_MPC_STATE"]["LKAS_Config"])
    lkas_config_isAccOn = (self.mpc_lkas_config != LKASConfig.DISABLE)
    lkas_isMainSwOn = bool(cp.vl["PCM_BUTTONS"]["BTN_TOGGLE_ACC_OnOff"])

    lkas_hud_AccOn1 = bool(cp_cam.vl["ACC_HUD_ADAS"]["AccOn1"])
    self.acc_state  = cp_cam.vl["ACC_HUD_ADAS"]["AccState"]
    self.adas_set_dist = cp_cam.vl["ACC_HUD_ADAS"]["SetDistance"]

    prev_btn_acc_cancel = self.btn_acc_cancel
    prev_btn_acc_set_reset = self.btn_acc_set_reset
    prev_btn_acc_dist_inc = self.btn_acc_dist_inc
    prev_btn_acc_dist_dec = self.btn_acc_dist_dec

    self.btn_acc_cancel = cp.vl["PCM_BUTTONS"]["BTN_AccCancel"]
    self.btn_acc_set_reset = cp.vl["PCM_BUTTONS"]["BTN_AccCancel"]
    self.btn_acc_dist_inc = cp.vl["PCM_BUTTONS"]["BTN_AccDistanceIncrease"]
    self.btn_acc_dist_dec = cp.vl["PCM_BUTTONS"]["BTN_AccDistanceDecrease"]

    # use wheels averages if you like
    # ret.wheelSpeeds = self.get_wheel_speeds(
    #     cp.vl["IPB"]["WheelSpeed_FL"],
    #     cp.vl["IPB"]["WheelSpeed_FR"],
    #     cp.vl["IPB"]["WheelSpeed_RL"],
    #     cp.vl["IPB"]["WheelSpeed_RR"],
    # )
    #speed_kph = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])

    # use dash speedo as speed reference
    speed_raw = int(cp.vl["CARSPEED"]["CarDisplaySpeed"])
    speed_raw_kph = speed_raw * CarControllerParams.K_DASHSPEED
    correct_factor = np.interp(speed_raw_kph, [30, 60, 90, 120], [1., 1., 1., 1.])
    self.speed_kph = speed_raw_kph * correct_factor

    ret.vEgoRaw = float(self.speed_kph * CV.KPH_TO_MS) # KPH to m/s
    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)

    ret.standstill = (speed_raw == 0)

    if self.CP.minSteerSpeed > 0:
      if self.speed_kph > 0.5:
        self.lkas_allowed_speed = True
      elif self.speed_kph < 0.1:
        self.lkas_allowed_speed = False
    else:
      self.lkas_allowed_speed = True

    can_gear = int(cp.vl["DRIVE_STATE"]["Gear"])
    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))

    ret.genericToggle = bool(cp.vl["STALKS"]["HeadLight"])
    if self.CP.enableBsm:
      ret.leftBlindspot = bool(cp.vl["BSD_RADAR"]["LEFT_APPROACH"])
      ret.rightBlindspot = bool(cp.vl["BSD_RADAR"]["RIGHT_APPROACH"])

    ret.leftBlinker = bool(cp.vl["STALKS"]["LeftIndicator"])
    ret.rightBlinker = bool(cp.vl["STALKS"]["RightIndicator"])

    ret.steeringAngleOffsetDeg = 0
    ret.steeringAngleDeg = cp.vl["EPS"]["SteeringAngle"]

    self.steeringRateDegAbs = cp.vl["EPS"]["SteeringAngleRate"]
    ret.steeringRateDeg = self.steeringRateDegAbs

    ret.steeringTorque = cp.vl["ACC_EPS_STATE"]["SteerDriverTorque"]
    ret.steeringTorqueEps = cp.vl["ACC_EPS_STATE"]["MainTorque"]
    self.eps_warning = bool(cp.vl["ACC_EPS_STATE"]["SteerWarning"]) #Todo: some firmware have SteerWarning field asserted.
    self.eps_state_counter = int(cp.vl["ACC_EPS_STATE"]["Counter"])

    ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > 59, 5)

    ret.parkingBrake = (cp.vl["EPB"]["EPB_ActiveFlag"] == 1)

    ret.brake =  int(cp.vl["PEDAL"]["BrakePedal"])
    ret.brakePressed = (ret.brake != 0)

    ret.seatbeltUnlatched = (cp.vl["BCM"]["DriverSeatBeltFasten"] != 1)

    ret.doorOpen = any([cp.vl["BCM"]["FrontLeftDoor"], cp.vl["BCM"]["FrontRightDoor"],
              cp.vl["BCM"]["RearLeftDoor"],  cp.vl["BCM"]["RearRightDoor"]])

    ret.gas = int(cp.vl["PEDAL"]["AcceleratorPedal"])
    ret.gasPressed = (ret.gas != 0)

    ret.cruiseState.available = lkas_isMainSwOn and lkas_config_isAccOn and lkas_hud_AccOn1
    ret.cruiseState.enabled = self.acc_state in (3, 5)
    ret.cruiseState.standstill = ret.standstill
    ret.cruiseState.speed = cp_cam.vl["ACC_HUD_ADAS"]["SetSpeed"] * CV.KPH_TO_MS

    #Todo: some firmware have these fields asserted.
    ret.steerFaultTemporary = bool((self.acc_state == 7) or self.eps_warning)

    self.acc_active_last = ret.cruiseState.enabled

    self.mpc_laks_output = cp_cam.vl["ACC_MPC_STATE"]["LKAS_Output"] #use to fool mpc
    self.mpc_laks_reqprepare = cp_cam.vl["ACC_MPC_STATE"]["LKAS_ReqPrepare"] != 0 #use to fool mpc
    self.mpc_laks_active = cp_cam.vl["ACC_MPC_STATE"]["LKAS_Active"] != 0 #use to fool mpc

    self.acc_hud_adas_counter = cp_cam.vl["ACC_HUD_ADAS"]["Counter"]
    self.acc_mpc_state_counter = cp_cam.vl["ACC_MPC_STATE"]["Counter"]
    self.acc_cmd_counter = cp_cam.vl["ACC_CMD"]["Counter"]

    self.cam_lkas = copy.copy(cp_cam.vl["ACC_MPC_STATE"])
    self.cam_adas = copy.copy(cp_cam.vl["ACC_HUD_ADAS"])
    self.cam_acc = copy.copy(cp_cam.vl["ACC_CMD"])
    self.esc_eps = copy.copy(cp.vl["ACC_EPS_STATE"])

    mrr_id = int(cp_cam.vl["RADAR_MRR"]["TargetID"])

    if mrr_id == 2: #1:left, 2:front, 3:right
      if bool(cp_cam.vl["RADAR_MRR"]["IsValid"]):
        self.mrr_leading_dist = int(cp_cam.vl["RADAR_MRR"]["LongDist"])
      else:
        self.mrr_leading_dist = 199

    ret.steerFaultPermanent = bool(cp.vl["ACC_EPS_STATE"]["TorqueFailed"]) #EPS give up all inputs until restart

    # if self.setTimeDelay == 0:
    #     if not system_time_valid():
    #         yyyy = int(cp.vl["DATETIME"]["YY"] + 2000)
    #         MM = int(cp.vl["DATETIME"]["MM"])
    #         DD = int(cp.vl["DATETIME"]["DD"])
    #         hh = int(cp.vl["DATETIME"]["hh"])
    #         mm = int(cp.vl["DATETIME"]["mm"])
    #         ss = int(cp.vl["DATETIME"]["ss"])
    #         china_time = datetime(yyyy,MM,DD,hh,mm,ss)
    #         china_utc_offset = timedelta(hours=8)
    #         utc_time = china_time - china_utc_offset
    #         cloudlog.debug(f"Setting time to {utc_time}")
    #         try:
    #             subprocess.run(f"TZ=UTC date -s '{utc_time}'", shell=True, check=True)
    #         except subprocess.CalledProcessError:
    #             cloudlog.exception("timed.failed_setting_time")
    # else:
    #     self.setTimeDelay = self.setTimeDelay - 1

    ret.buttonEvents = [
      *create_button_events(self.btn_acc_cancel, prev_btn_acc_cancel, {1: ButtonType.cancel}),
      *create_button_events(self.btn_acc_set_reset, prev_btn_acc_set_reset, {1: ButtonType.decelCruise, 3: ButtonType.accelCruise}),
      *create_button_events(self.btn_acc_dist_inc, prev_btn_acc_dist_inc, {1: ButtonType.gapAdjustCruise}),
      *create_button_events(self.btn_acc_dist_dec, prev_btn_acc_dist_dec, {1: ButtonType.gapAdjustCruise}),
    ]

    return ret


  @staticmethod
  def get_can_parsers(CP):
    pt_messages = [
      # sig_address, frequency
      ("EPS", 100),
      ("CARSPEED", 50),
      ("PEDAL", 50),
      ("EPB", 1),
      ("ACC_EPS_STATE", 50),
      ("DRIVE_STATE", 50),
      ("STALKS", 1),
      ("BCM", 1),
      ("PCM_BUTTONS", 20),
      ("DATETIME", 2),
    ]

    if CP.enableBsm:
      pt_messages.append(("BSD_RADAR", 20))

    cam_messages = [
      ("ACC_HUD_ADAS", 50),
      ("ACC_CMD", 50),
      ("ACC_MPC_STATE", 50),
      ("RADAR_MRR", 60),
    ]

    return {
      Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus.ESC),
      Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus.MPC),
    }